#pragma once
#ifndef COLLLISION_DETECTOR_H
#define COLLLISION_DETECTOR_H

#include "basic_typedef.h"
#include <CGAL/squared_distance_2.h>
#include <CGAL/minkowski_sum_2.h>

class CollisionDetector
{
public:
	
	////////////////////////
	// CTORS
	////////////////////////
	/*	Parameters:
			radius	- radius of robot
			Room	- describes the boundary of the scenario
	*/
	CollisionDetector(double radius, Obstacles *obs): 
				m_radius(radius), m_obs(obs) 
	{
		//	Approximates the disc robot with a polygon
		Polygon_2   approx_robot;

		/*	TODO: generate the approx_robot polygon.
			Below is an example of a coarse approximation:*/

		Point_2		p1(-radius*1.5, -radius*1.5);
		Point_2		p2(-radius*1.5, radius*1.5);
		Point_2		p3(radius*1.5, radius*1.5);
		Point_2		p4(radius*1.5, -radius*1.5);

		approx_robot.push_back(p1);
		approx_robot.push_back(p2);
		approx_robot.push_back(p3);
		approx_robot.push_back(p4);

		// Construction of the polygon set, used for collision detection
		if (!m_obs->empty())
		{
			for (Obstacles::iterator iter = m_obs->begin(); iter != m_obs->end(); iter++)
			{
				// For every obstacle calculate its Minkowski sum with the "robot"
				Polygon_with_holes_2	poly_wh = minkowski_sum_2(*iter, approx_robot);

				// Add the result to the polygon set
				m_poly_set.join(poly_wh);
			}
		}
	}
	
	~CollisionDetector(){};

	////////////////////////
	// Queries
	////////////////////////
	
	/*	Check if the given configuration is collision free (returns true for free samples) */
	bool	valid_conf(Point_2 pos)
	{
		return (m_poly_set.oriented_side(pos) == CGAL::ON_NEGATIVE_SIDE);
	}

	/*	Validate the connection between two configurations by 
		sampling along a line.
		Input:	* Start and target configurations
				* Eps value that determines the size of the steps 
					that are taken along the line
	*/
	bool	local_planner(Point_2 start, Point_2 target, double eps)
	{
		double x1 = start.x();
		double y1 = start.y() ;
		double x2 = target.x() ;
		double y2 = target.y() ;

		double distance = sqrt(pow(x1 - x2, 2) + pow(y1 - y2,2));

		double step_size = eps;

		// calculate how many steps are required
		int step_num = floor((distance - step_size) / step_size);
		double vx = x2 - x1;
		double vy = y2 - y1;

		for (int i = 1; i <= step_num; i++)
		{
			// generate a configuration for every step
			double offset =  (i * step_size) / (distance - step_size);
			double currx = x1 + vx * offset;
			double curry = y1 + vy * offset;

			Point_2 currentPos(currx, curry);

			// If an intermidiate configuration is invalid, return false
			if (!valid_conf(currentPos))
				return false;
		}

		// GREAT SUCCESS!
		return true;
	}

	////////////////////////
	// Data Members
	////////////////////////
	double			m_radius;	// radius of robot
	Obstacles		*m_obs;		// Collection of polygonal obstacles
	Polygon_set_2	m_poly_set; // Polygon set, for collision detection
};

#endif